import java.io.*;
import josx.rcxcomm.*;
import josx.platform.rcx.*;
import josx.robotics.*;
import java.util.Vector;
import java.lang.Integer;

/** Very simple example of using RCXInputStream on the RCX
 * Communicates with Write.java on the PC
 * For 2-way communication use RCXPort
 */
 
public class PathExecutor {
  private static boolean lost = false;

  public static void main (String [] args) throws IOException {
	///Initializes sensor
	Sensor.S2.setTypeAndMode (3, 0x80);
	Sensor.S2.activate();
	Sensor.S2.addSensorListener(new SensorListener() {
		public void stateChanged(Sensor aSource, int aOldValue, int aNewValue) {
			// respond to sensor value change here
			LCD.showNumber(aNewValue);
			if (aNewValue >= 25 && aNewValue <= 29) {
				lost = true;
			}
		}
	});
	
	///Vars
    RCXInputStream in = new RCXInputStream();
	char[] data = new char[300];
    int count = 0;
    
    while(in.available() == 0) {}
    
	char current = 0;
	while (current != 255) {
		//LCD.showNumber(in.read());
		while (in.available() == 0) {}
		current = (char)in.read();
		data[count] = current;
		count++;
        //pen.append(current);
	}
	
	count--;
    
    int[] x_pathLine = new int[150];
	int[] y_pathLine = new int[150];
	
	int xCount = 0 , yCount = 0;
	
	for (int i = 0 ; i < count ; i++) {
		LCD.showNumber(data[i]);
		delay(200);
		if (i % 2 == 0) {
			x_pathLine[xCount] = (int) data[i];
			xCount++;
		}
		else {
			y_pathLine[yCount] = (int) data[i];
			yCount++;
		}
	}
	
	for (int i = 0 ; i < xCount ; i++) {
		/*LCD.showNumber(x_pathLine[i]);
		delay(500);
		LCD.showNumber(y_pathLine[i]);
		delay(500);*/
	}
	
	
	int moveDelay , rotateDelay , correctionDelay , lDelay , rDelay, lPower , rPower;
	boolean preference , hybrid = false;
	moveDelay = x_pathLine[0] * 10;
	rotateDelay = y_pathLine[0] * 10;
	correctionDelay = x_pathLine[1] * 10;
	preference = (y_pathLine[1] == 1);
	hybrid = (y_pathLine[1] == 2);
	lDelay = x_pathLine[2] * 10;
	rDelay = y_pathLine[2] * 10;
	lPower = x_pathLine[3];
	rPower = y_pathLine[3];
	
	int currentX , currentY , prevX , prevY;
	int dirX , dirY , prevDirX = 1 , prevDirY = 0;
	Motor.A.setPower(lPower);
	Motor.C.setPower(rPower);
	
	lost = false;
	for (int i = 5 ; i < xCount ; i++) {
		currentX = x_pathLine[i];
		currentY = y_pathLine[i];
		
		prevX = x_pathLine[i-1];
		prevY = y_pathLine[i-1];
		
		if (prevX == currentX && prevY == currentY) continue;
		
		dirX = (currentX != prevX) ? ((currentX > prevX) ? 1 : -1) : 0;
		dirY = (currentY != prevY) ? ((currentY > prevY) ? 1 : -1) : 0;
		
		if (prevDirX * dirX == -1 || prevDirY * dirY == -1) {
			///U-Turn
			left();
			delay(lDelay * 2);
			stop();
		}
		else if (prevDirX * dirY - prevDirY * dirX > 0) {
			///Right Turn
			right();
			delay(rDelay);
			stop();
		}
		else if (prevDirX * dirY - prevDirY * dirX < 0) {
			///Left Turn
			left();
			delay(lDelay);
			stop();
		}
		delay(750);
		
		//TimingNavigator navigator = new TimingNavigator(Motor.C , Motor.A , 20.00F , 4.2F);
		//navigator.gotoPoint(xVal , yVal);
		///Move
		go();
		delay(moveDelay);
		stop();
		
		///Recovery
		if (lost) {
			///Backward
			back();
			delay(moveDelay);
			stop();
			
			///Correction
			if (preference)
				left();
			else
				right();
			delay(correctionDelay);
			stop();
			
			///Move
			lost = false;
			go();
			delay(moveDelay);
			stop();
			
			///Recorrecting
			if (hybrid) {
				if (lost) { ///again???
					///Backward
					back();
					delay(moveDelay);
					stop();
					
					///Correction
					if (preference)
						right();
					else
						left();
					delay(correctionDelay * 2);
					stop();
					
					///Move
					lost = false;
					go();
					delay(moveDelay);
					stop();
				}
			}
		
			lost = false;
		}
		
		prevDirX = dirX;
		prevDirY = dirY;
		delay(750);
	}
    
  }
  
  private static void right() {
	Motor.A.forward();
	Motor.C.backward();
  }
  
  private static void left() {
	Motor.C.forward();
	Motor.A.backward();
  }
  
  private static void go() {
	Motor.C.forward();
	Motor.A.forward();
  }
  
  private static void back() {
	Motor.C.backward();
	Motor.A.backward();
  }
  
  private static void stop() {
	Motor.A.stop();
	Motor.C.stop();
  }
  
  private static void delay(int time) {
	try {
		  Thread.sleep(time);
	}
	catch (InterruptedException e) {}
  }
  
  private static float parseFloat(String val) {
	boolean beforeComma = true;
	return 0.0F;
  }
  
  
  
}

